// system
#include <math.h>
#include <float.h>
// local
#include "gtrack_int.h"

#ifdef GTRACK_2D

void gtrack_sph2cart(GTRACK_measurement_vector* v, GTRACK_cartesian_position* c)
{
    float sinAngle, cosAngle;

    gtrack_sincosd(DEG(v->angle), &sinAngle, &cosAngle);
    c->posX = v->range * sinAngle;
    c->posY = v->range * cosAngle;
}

void gtrack_calcMeasurementLimits(float range, GTRACK_gateLimits* gate_limits, GTRACK_measurement_vector* limits)
{
    if (gate_limits->depth <= FLT_MIN)
        limits->range = FLT_MAX;
    else
        limits->range = gate_limits->depth / 2;

    if (gate_limits->width <= FLT_MIN)
        limits->angle = FLT_MAX;
    else
        limits->angle = atanf((gate_limits->width / 2) / range);

    if (gate_limits->vel <= FLT_MIN)
        limits->doppler = FLT_MAX;
    else
        limits->doppler = gate_limits->vel / 2;
}

void gtrack_calcDim(float* mSpread, float R, float* tDim)
{
    tDim[0] = mSpread[0];
    tDim[1] = 2 * R * tanf(mSpread[1] / 2);
    tDim[2] = mSpread[2];
}

float gtrack_calcDistance(GTRACK_measurement_vector* p1, GTRACK_measurement_vector* p2)
{
    // d = u1(1)^2 + u2(1)^2 - 2*u1(1)*u2(1)*cos(u1(2)-u2(2));
    float sinAngle, cosAngle;
    gtrack_sincosd(DEG((p1->angle - p2->angle)), &sinAngle, &cosAngle);
    return p1->range * p1->range + p2->range * p2->range - 2 * p1->range * p2->range * cosAngle;
}

void gtrack_matrixComputePJT(float* P, float* J, float* PJ)
{
    /* We compute PJ' first because it is re-used in Kalman Gain calculations */
    uint16_t i, j, k, n;
    /* PJ[6x3] = P[6x6] x J[3x6]' */
    for (i = 0U, k = 0U; i < 18U; i += 3U, k += 6U)
    {
        for (j = 0U, n = 0U; j < 3U; j++, n += 6U)
        {
            PJ[i + j] = (P[k + 0U] * J[n + 0U]) +
                (P[k + 1U] * J[n + 1U]) +
                (P[k + 2U] * J[n + 2U]) +
                (P[k + 3U] * J[n + 3U]) +
                (P[k + 4U] * J[n + 4U]) +
                (P[k + 5U] * J[n + 5U]);
        }
    }
}

/**
 *  @b Description
 *  @n
 *		该函数用于对两个大小为 6x6 的矩阵进行乘法运算。
 *		矩阵都是实数，单精度浮点运算。
 *		矩阵是row-major顺序。
 *
 *  @param[in]  A
 *		Matrix A
 *  @param[in]  B
 *		Matrix B
 *  @param[out]  C
 *		Matrix C(6x6x) = A(6x6) X B(6x6)
 *  @retval
 *      None
 */

void gtrack_matrixMultiply66(float* A, float* B, float* C)
{
    uint16_t i, j;
    for (i = 0U; i < 36U; i += 6U)
    {
        for (j = 0U; j < 6U; j++)
        {
            C[i + j] = (A[i + 0U] * B[j + 0U]) +
                (A[i + 1U] * B[j + 6U]) +
                (A[i + 2U] * B[j + 12U]) +
                (A[i + 3U] * B[j + 18U]) +
                (A[i + 4U] * B[j + 24U]) +
                (A[i + 5U] * B[j + 30U]);
        }
    }
}

/**
 *  @b Description
 *  @n
 *		该函数用于对两个大小为 6x6 的矩阵进行乘法运算。
 *		第二个矩阵首先被转置
 *		矩阵都是实数，单精度浮点运算。
 *		矩阵是row-major顺序。
 *
 *  @param[in]  A
 *		Matrix A
 *  @param[in]  B
 *		Matrix B
 *  @param[out]  C
 *		Matrix C(6x6) = A(6x6) X B(6x6)T
 *  @retval
 *      None
 */

void gtrack_matrixMultiply66T(float* A, float* B, float* C)
{
    uint16_t i, j, k;
    for (i = 0; i < 36; i += 6)
    {
        for (j = 0U, k = 0U; j < 6U; j++, k += 6U)
        {
            C[i + j] = (A[i + 0U] * B[k + 0U]) +
                (A[i + 1U] * B[k + 1U]) +
                (A[i + 2U] * B[k + 2U]) +
                (A[i + 3U] * B[k + 3U]) +
                (A[i + 4U] * B[k + 4U]) +
                (A[i + 5U] * B[k + 5U]);
        }
    }
}

/**
 *  @b Description
 *  @n
 *		此函数计算 3x3 矩阵的行列式。
 *		矩阵为实数，单精度浮点运算。
 *		矩阵是row-major顺序。
 *
 *  @param[in]  A
 *		Matrix A
 *  @param[out]  det
 *		det = det(A);
 *  @retval
 *      None
 */
void gtrack_matrixDet(float* A, float* det)
{
    *det = A[0] * (A[4] * A[8] - A[7] * A[5]) -
        A[1] * (A[3] * A[8] - A[5] * A[6]) +
        A[2] * (A[3] * A[7] - A[4] * A[6]);
}

void gtrack_matrixInv(const float* A, float* det_out, float* inv)
{
    float det;
    float invdet;

    det = A[0] * (A[4] * A[8] - A[7] * A[5]) -
        A[1] * (A[3] * A[8] - A[5] * A[6]) +
        A[2] * (A[3] * A[7] - A[4] * A[6]);

    invdet = 1 / det;

    inv[0] = (A[4] * A[8] - A[7] * A[5]) * invdet;
    inv[1] = (A[2] * A[7] - A[1] * A[8]) * invdet;
    inv[2] = (A[1] * A[5] - A[2] * A[4]) * invdet;
    inv[3] = (A[5] * A[6] - A[3] * A[8]) * invdet;
    inv[4] = (A[0] * A[8] - A[2] * A[6]) * invdet;
    inv[5] = (A[3] * A[2] - A[0] * A[5]) * invdet;
    inv[6] = (A[3] * A[7] - A[6] * A[4]) * invdet;
    inv[7] = (A[6] * A[1] - A[0] * A[7]) * invdet;
    inv[8] = (A[0] * A[4] - A[3] * A[1]) * invdet;

    *det_out = det;
}

void gtrack_computeMahalanobis(float* v, float* D, float* md)
{
    *md =
        v[0] * (v[0] * D[0] + v[1] * D[3] + v[2] * D[6]) +
        v[1] * (v[0] * D[1] + v[1] * D[4] + v[2] * D[7]) +
        v[2] * (v[0] * D[2] + v[1] * D[5] + v[2] * D[8]);
}

void gtrack_computeMahalanobisPartial(float* v, float* D, float* mdp)
{
    *mdp =
        v[0] * (v[0] * D[0] + v[1] * D[3]) +
        v[1] * (v[0] * D[1] + v[1] * D[4]);
}


uint8_t gtrack_isPointBehindTarget(GTRACK_measurement_vector* p, GTRACK_measurement_vector* target)
{
    if ((fabsf(p->angle - target->angle) < RAD(2)) && (p->range > target->range))
        return 1U;
    else
        return 0;
}

uint8_t gtrack_isPointInsideBox(GTRACK_cartesian_position* c, GTRACK_boundaryBox* box)
{
    if ((c->posX > box->x1) && (c->posX < box->x2) &&
        (c->posY > box->y1) && (c->posY < box->y2))
        return 1U;
    else
        return 0;
}
#endif
